import math
from dubins import Dubins_path
import numpy as np
import pandas as pd
import matplotlib.pyplot as plt
import random
from math import  sin, cos
import Cantor
from tqdm import trange, tqdm


class FWDM:
    def __init__(self):
        self.TaskLine = []  # 任务段
        self.df = pd.read_csv("distance/point.csv", encoding="gbk")
        self.df_array = np.array(self.df)
        self.df_list = self.df_array.tolist()
        for i in self.df_list:
            fs = [i[3] * 100, i[4] * 100]
            fe = [i[5] * 100, i[6] * 100]
            es = [i[7] * 100, i[8] * 100]
            ee = [i[9] * 100, i[10] * 100]
            self.TaskLine.append([fs, fe])
        # 作图
        # 生成距离矩阵
        self.mis_points = [58.89 * 100, 19.26 * 100, math.pi]  # 加入起飞点
        self.airport_misspoint10 = []
        self.airport_misspoint11 = []
        self.misspoint_airport01 = []
        self.misspoint_airport11 = []
        self.misspoint_misspoint00 = []
        self.misspoint_misspoint01 = []
        self.misspoint_misspoint10 = []
        self.misspoint_misspoint11 = []
        tes = pd.read_csv("distance/airport.csv")
        tes = tes.values
        self.airport_misspoint10 = tes[0]
        self.airport_misspoint11 = tes[1]
        self.misspoint_airport01 = tes[2]
        self.misspoint_airport11 = tes[3]
        self.misspoint_misspoint00 = pd.read_csv("distance/length_00.csv").values
        self.misspoint_misspoint01 = pd.read_csv("distance/length_01.csv").values
        self.misspoint_misspoint10 = pd.read_csv("distance/length_10.csv").values
        self.misspoint_misspoint11 = pd.read_csv("distance/length_11.csv").values
        # 参数设置
        self.lb = 0  # 下界
        self.ub = 14  # 上界
        self.dim = 14
        self.H = 6
        self.RES = []
        self.SearchAgents_no = math.ceil(75 * (self.dim ** (2 / 3))) // 2
        # SearchAgents_no =3
        self.X = []  # 时序蔟
        self.Y = []  # 方向蔟
        self.MV = []  # 变异幅度蔟
        self.CV = []  # 交叉率
        self.Z = [0] * self.SearchAgents_no  # 路径长度记录
        self.FEsmax = 10000 * self.dim  # 最大迭代次数
        # FEsmax = 200
        self.Optimal = [math.inf] * self.H  # 最优路径长度
        self.H_X = [[math.inf] * self.dim] * self.H
        self.H_Y = [[math.inf] * self.dim] * self.H
        self.H_Z = [math.inf] * self.H
        self.H_idx = [math.inf] * self.H
        self.Records = []
        self.R_X = []
        self.R_Z = math.inf
        self.R_T = 0
        # 初始化种群
        tmp = list(range(self.lb, self.ub))
        # 7:3
        sum = Cantor.getFactorial(self.dim)
        scale = 3
        X_SUM = self.SearchAgents_no // scale
        Y_SUM = self.SearchAgents_no // X_SUM
        for i in range(X_SUM):
            for j in range(scale):
                self.X.append(Cantor.DeCantor(self.dim, i * (sum // X_SUM)))
            for i in range(Y_SUM):
                self.Y.append(Cantor.GetBin(self.dim, i * (2 ** self.dim // Y_SUM)))
        while len(self.CV) < self.SearchAgents_no:  # 生交叉幅度蔟
            self.CV.append(int(random.uniform(0.1, 1) * self.dim))
        while len(self.MV) < self.SearchAgents_no:  # 生变异幅度蔟
            self.MV.append(int(random.uniform(0.1, 1) * self.dim))
        self.M_type = 1
    def list2number(self,mylist = []):
        s_c = ""
        for i in mylist:
            s_c += str(i)
        c = int(s_c, 2)  # change
        if c > 2 ** self.dim:  # 防止越界
            c = 2 ** self.dim
        return c
    def number2list(self,number = 0):
        c = bin(number)
        tmp_y = []
        for i in c[2:]:
            tmp_y.append(int(i))
        while len(tmp_y) < self.dim:
            tmp_y.insert(0,0)
        return tmp_y.copy()

    def Update_Z(self):
        for i in range(len(self.X)):
            pathlenth = 0
            P = self.X[i]
            # 机场-首点
            if self.Y[i][0] == 0:  # 首点左侧进
                pathlenth += self.airport_misspoint10[int(P[0])]
            else:  # 首点右侧进
                pathlenth += self.airport_misspoint11[int(P[0])]
            # 点-点
            for j in range(len(P) - 1):
                s = P[j]
                e = P[j + 1]
                if self.Y[i][j] == 0 and self.Y[i][j + 1] == 0:
                    pathlenth += self.misspoint_misspoint00[s][e]
                if self.Y[i][j] == 0 and self.Y[i][j + 1] == 1:
                    pathlenth += self.misspoint_misspoint01[s][e]
                if self.Y[i][j] == 1 and self.Y[i][j + 1] == 0:
                    pathlenth += self.misspoint_misspoint10[s][e]
                if self.Y[i][j] == 1 and self.Y[i][j + 1] == 1:
                    pathlenth += self.misspoint_misspoint11[s][e]
            # 末点-机场
            if self.Y[i][-1] == 0:  # 末点左侧进 右侧出
                pathlenth += self.misspoint_airport01[int(P[-1])]
            else:  ##末点右侧进 左侧出
                pathlenth += self.misspoint_airport11[int(P[-1])]
            self.Z[i] = pathlenth
    def Update_HZ(self):
        for i in range(len(self.H_X)):
            pathlenth = 0
            P = self.H_X[i]
            if math.inf in P:
                continue
            # 机场-首点
            if self.H_Y[i][0] == 0:  # 首点左侧进
                pathlenth += self.airport_misspoint10[int(P[0])]
            else:  # 首点右侧进
                pathlenth += self.airport_misspoint11[int(P[0])]
            # 点-点
            for j in range(len(P) - 1):
                s = P[j]
                e = P[j + 1]
                if self.H_Y[i][j] == 0 and self.H_Y[i][j + 1] == 0:
                    pathlenth += self.misspoint_misspoint00[s][e]
                if self.H_Y[i][j] == 0 and self.H_Y[i][j + 1] == 1:
                    pathlenth += self.misspoint_misspoint01[s][e]
                if self.H_Y[i][j] == 1 and self.H_Y[i][j + 1] == 0:
                    pathlenth += self.misspoint_misspoint10[s][e]
                if self.H_Y[i][j] == 1 and self.H_Y[i][j + 1] == 1:
                    pathlenth += self.misspoint_misspoint11[s][e]
            # 末点-机场
            if self.H_Y[i][-1] == 0:  # 末点左侧进 右侧出
                pathlenth += self.misspoint_airport01[int(P[-1])]
            else:  ##末点右侧进 左侧出
                pathlenth += self.misspoint_airport11[int(P[-1])]
            self.Z[i] = pathlenth
    def Update_H(self):#保留先进先出原则 保持差异性  最优解保留
        sum_x = self.H_X.copy() + self.X.copy()
        sum_y = self.H_Y.copy() + self.Y.copy()
        sum_z = self.H_Z.copy() + self.Z.copy()
        sum_indx = list(range(len(sum_x)))
        index_price = zip(sum_indx.copy(), sum_z.copy())
        sorted_index_price = sorted(index_price, key=lambda x: x[1])
        # 将排序好的列表分出来并打印
        result = zip(*sorted_index_price)
        sorted_index, sorted_Z = [list(x) for x in result]
        self.H_X = []
        self.H_Z = []
        self.H_Y = []
        min_idx = 0
        while len(self.H_X) < 6 and min_idx < len(sorted_index):
            if sum_z[sorted_index[min_idx]] not in self.H_Z:
                if sum_x[sorted_index[min_idx]] not in self.H_X:
                    self.H_X.append(sum_x[sorted_index[min_idx]])
                    self.H_Y.append(sum_y[sorted_index[min_idx]])
                    self.H_Z.append(sum_z[sorted_index[min_idx]])
                elif self.H_Y[self.H_X.index(sum_x[sorted_index[min_idx]])] != sum_y[sorted_index[min_idx]]:
                    self.H_X.append(sum_x[sorted_index[min_idx]])
                    self.H_Y.append(sum_y[sorted_index[min_idx]])
                    self.H_Z.append(sum_z[sorted_index[min_idx]])
                else:
                    pass
            min_idx += 1

    def Cross(self):
        # 交叉  截取六组优秀组合中的一段 按照V中大小 替换拼接到待交叉蔟中
        for i in range(len(self.X)):
            tmp_x = self.X[i].copy()
            tmp_y = self.Y[i].copy()
            cross_rate = self.CV[i]  # 交叉率
            s = random.randint(0, self.dim)  # 起始点
            # 段长和起点
            tmp_optimal_x = random.choice(self.H_X)  # 随机从最优序列镞中选择一组
            tmp_optimal_y = self.H_Y[self.H_X.index(tmp_optimal_x)].copy()
            cut_optimal_X = []
            cut_optimal_Y = []
            for j in range(s, s + cross_rate):  # 获得截取序列
                cut_optimal_X.append(tmp_optimal_x[j % len(tmp_optimal_x)])
                cut_optimal_Y.append(tmp_optimal_y[j % len(tmp_optimal_y)])
            for k in range(len(cut_optimal_X)):
                n_x = cut_optimal_X[k]
                n_y = cut_optimal_Y[k]
                o = tmp_x.index(n_x)  # x与y共用 origin
                tmp_n = tmp_x[s % len(tmp_optimal_x)]
                tmp_x[s % len(tmp_optimal_x)] = n_x
                tmp_x[o] = tmp_n

                tmp_n = tmp_y[s % len(tmp_optimal_x)]
                tmp_y[s % len(tmp_optimal_x)] = n_y
                tmp_y[o] = tmp_n
                s += 1
            self.X[i] = tmp_x.copy()
            self.Y[i] = tmp_y.copy()
    def Mutation(self,case = 1):
        self.M_type = case
        if case == 1:
            for mi in range(len(self.X)):
                tmp_x = self.X[mi].copy()
                tmp_y = self.Y[mi].copy()
                cross_rate = self.MV[mi]  # 交叉率
                Frequency = cross_rate
                for f in range(Frequency):
                    s = random.randint(0, len(tmp_x) - 1)  # start
                    e = random.randint(0, len(tmp_x) - 1)  # end
                    tmp = tmp_x[s]
                    tmp_x[s] = tmp_x[e]
                    tmp_x[e] = tmp
                    c = random.randint(0, len(tmp_y) - 1)  # change
                    tmp_y[c] = (tmp_y[c] + 1) % 2
                self.X[mi] = tmp_x.copy()
                self.Y[mi] = tmp_y.copy()
        # 方案二 二进制转数值计算相似性变异
        if case == 2:
            for i in range(len(self.X)):
                tmp_x = self.X[i].copy()
                tmp_y = self.Y[i].copy()
                cross_rate = self.MV[i]  # 交叉率
                Frequency = random.randint(0, cross_rate)
                for f in range(Frequency):
                    s = random.randint(0, len(tmp_x) - 1)  # start
                    e = random.randint(0, len(tmp_x) - 1)  # end
                    tmp = tmp_x[s]
                    tmp_x[s] = tmp_x[e]
                    tmp_x[e] = tmp
                number = self.list2number(tmp_y)
                number += int(-1 * random.randint(0, 1) * Frequency)
                if number < 0:
                    number = 0
                if number > 2 ** self.dim:
                    number = 2 ** self.dim
                tmp_y = self.number2list(number)
                self.X[i] = tmp_x.copy()
                self.Y[i] = tmp_y.copy()
        # 方案三 在方案二基础上 使方向序列向最优序列探索 时序已打乱 方向靠近无意义
        if case == 3:
            for i in range(len(self.X)):
                tmp_x = self.X[i].copy()
                tmp_y = self.Y[i].copy()
                cross_rate = self.MV[i]  # 交叉率
                Frequency = random.randint(0, cross_rate)
                if random.randint(0, 50) % 2 == 0:  #
                    for f in range(Frequency):
                        s = random.randint(0, len(tmp_x) - 1)  # start
                        e = random.randint(0, len(tmp_x) - 1)  # end
                        tmp = tmp_x[s]
                        tmp_x[s] = tmp_x[e]
                        tmp_x[e] = tmp
                else:
                    number = self.list2number(tmp_y)
                    listchoice = [-1, 1]
                    number += int(random.choice(listchoice) * (Frequency / self.dim) * number)
                    if number < 0:
                        number = 0
                    if number > 2 ** self.dim:
                        number = int(2 ** self.dim) - 1

                    tmp_y = self.number2list(number)
                self.X[i] = tmp_x.copy()
                self.Y[i] = tmp_y.copy()
        if case == 4:#基于汉明距离的变异 感觉不是很合理 但又有点合理 只是不太合理 硬说合理其实也合理
            for i in range(len(self.X)):
                tmp_x = self.X[i].copy()
                tmp_y = self.Y[i].copy()
                cross_rate = self.MV[i]  # 交叉率
                Frequency = random.randint(0, cross_rate)
                if random.randint(0, 50) % 2 == 0:  #
                    for f in range(Frequency):
                        s = random.randint(0, len(tmp_x) - 1)  # start
                        e = random.randint(0, len(tmp_x) - 1)  # end
                        tmp = tmp_x[s]
                        tmp_x[s] = tmp_x[e]
                        tmp_x[e] = tmp
                else:
                    for f in range(Frequency):
                        c = random.randint(0, len(tmp_y) - 1)  # change
                        tmp_y[c] = (tmp_y[c] + 1) % 2
                self.X[i] = tmp_x.copy()
                self.Y[i] = tmp_y.copy()
    def Adjust(self,FEs):#根据迭代次数优化
        for i in range(len(self.MV)):
            if FEs < 0.6 * self.FEsmax:
                self.MV[i] = min(self.MV[i],int(0.7 * self.dim))
            if FEs < 0.25 * self.FEsmax:
                self.CV[i] = max(self.CV[i], int(0.7 * self.dim))
            elif FEs < 0.5 * self.FEsmax:
                self.CV[i] = max(self.CV[i], int(0.6 * self.dim))
    def Run(self):
        self.Update_Z()
        self.Update_H()
        bar = tqdm(range(self.FEsmax))
        for FEs in bar:
            self.Cross()
            self.Update_Z()
            self.Update_H()
            self.Mutation(1)  # 1单纯点位变异  2时序及方向同时变异 3时序和方向随机选一种变异 4基于汉明距离的变化
            self.Update_Z()
            self.Update_H()
            self.Adjust(FEs)
            first_record = False
            for rec in range(len(self.H_X)):
                if first_record == False:
                    self.RES_X = self.H_X[rec]
                    self.RES_Y = self.H_Y[rec]
                    self.RES_Z = self.H_Z[rec]
                    first_record = True
            if self.R_Z == self.RES_Z:
                self.R_T += 1
                if self.R_T == 300:
                    break
            else:
                self.R_T = 0
                self.R_Z = self.RES_Z
            self.Records.append(sorted(self.H_Z)[0])
            bar.set_description("Convergent %s" % (300 - self.R_T))  # 自定义提示信息
            bar.set_postfix(RES_X = self.RES_X, RES_Y = self.RES_Y,RES_Z = int(self.RES_Z))
        print("RES:", self.RES_X, ":", self.RES_Y, ":", self.RES_Z)
        return self.RES_X,self.RES_Y
